#include "FixPoint.h"

namespace common {

FixPoint::FixPoint(int id, Point *k, Vector *vl) :
	common::Constraint(id, 2, FIX_POINT) {
	setK(k);
	setVectorL(vl);

}

void FixPoint::setVectorL(Vector * _vl) {
	vl = _vl;
}
const Vector* FixPoint::getVectorL(void) {
	return vl;
}

ublasVector FixPoint::getValue() throw (MyException) {
	// K-L
	ublasVector v(2);
	v << ((*getK()) - (*getVectorL()));
	return v;
}

ublasMatrix FixPoint::getJacobianOnPoint(Point* p) throw (MyException) {
	if (getK() == p) {
		return ublasMatrixIdentity(2,2);
	} else {
		return ublasMatrixZero(2,2);
	}
}
std::string FixPoint::toString(void) {
	std::stringstream o;
	if (getVectorL() != NULL) {
		o << "Constraint-" << GeometricConstraintTypeNames[getType()] << "-"
				<< this->getId() << "- [";
		if (getK() != NULL)
			o << " p" << getK()->getId() << "-(" << getK()->getX() << ","
					<< getK()->getY() << ")" << " ,";
		o << " v-(" << getVectorL()->getX() << "," << getVectorL()->getY()
				<< ")" << " ";
		o << "]" ;
	} else {
		o << Constraint::toString();
	}
	return o.str();
}
FixPoint::~FixPoint() {
	delete vl;
}

}

